#include "stdafx.h" 
#include "motor.hpp"
#include <stdio.h>
#include <time.h>
#include <string>
#include <stdlib.h>
using namespace std;

    
    Motor::Motor(){
    }
    Motor::Motor(int output, Serial *cp){
        port=output;
        comport=cp;
        mode = BREAK || REGULATED;
        reg_mode = MOTOR_SPEED;
        run_state = RUN_IDLE;
        turnratio = 100;
    }
    
    int Motor::get_output_state(){
        char *answer;
        char command[5];
        command[0]=0x03;  //command length
        command[1]=0x00;
        
        //start of message
        command[2]=0x00;
        command[3]=0x06;
        command[4]=port;
        comport->send(&command[0],5);    
        answer=comport->receive(27);
        //mode=answer[7];
        //reg_mode=answer[8];
        //turnratio=answer[9];
        run_state=answer[10];
        tacho_limit = (0xff & answer[11]) | ((0xff & answer[12]) << 8)| ((0xff & answer[13]) << 16)| ((0xff & answer[14]) << 24);
        tacho_count = (0xff & answer[15]) | ((0xff & answer[16]) << 8)| ((0xff & answer[17]) << 16)| ((0xff & answer[18]) << 24);
        block_count = (0xff & answer[19]) | ((0xff & answer[20]) << 8)| ((0xff & answer[21]) << 16)| ((0xff & answer[22]) << 24);
        rotation_count = (0xff & answer[19]) | ((0xff & answer[20]) << 8)| ((0xff & answer[21]) << 16)| ((0xff & answer[22]) << 24);
        //printf("Run state: %d\n", run_state);
        //printf("Tacho limit: %d\n", tacho_limit);
        //printf("Tacho count: %d\n", tacho_count);
        //printf("Block tacho count: %d\n", block_count);
        //printf("Rotation count: %d\n", rotation_count);
        //printf("\n");
        return 1;
    }
    
    
    int Motor::set_output_state(int set_speed, int set_mode, int set_regulation, int set_turn_ratio,int set_run_state, long unsigned int set_tacho_limit){
         char command[15];
         command[0]=13;  //command length
         command[1]=0x00;

         //start of message
         command[2]=0x80;
         command[3]=0x04;//Turn on motor

         command[4]=port;//Output port
         command[5]=set_speed;//power set point
         command[6]=set_mode;//Mode

         command[7]=set_regulation;//Regulation
         command[8]=set_turn_ratio;//Turn ratio

         command[9]=set_run_state;//Run stat
         
         command[10]=set_tacho_limit;//TachoLimit
         command[11]=set_tacho_limit >> 8;//TachoLimit
         command[12]=set_tacho_limit >> 16;//TachoLimit
         command[13]=set_tacho_limit >> 24;//TachoLimit
         command[14]=0;                   // why a 5th byte????? 
         return comport->send(&command[0],15);
    }
    
    int Motor::reset(bool relative){
        char command[6];
        command[0]=0x04;  //command length
        command[1]=0x00;

        //start of message
        command[2]=0x80;
        command[3]=0x0A;
        command[4]=port;
        command[5]=relative;
        return comport->send(&command[0],6);    
    }
    
    
    
    int Motor::on(int speed){
        return set_output_state(speed, BREAK | MOTOR_ON | REGULATED, reg_mode, turnratio, RUNNING, 0);
    }
    
    int Motor::on(int speed, long unsigned int degrees){
        return set_output_state(speed, BREAK | MOTOR_ON | REGULATED, reg_mode, turnratio, RUNNING, degrees);
    }

    int Motor::stop(){
        return set_output_state(0, BREAK | MOTOR_ON | REGULATED, reg_mode, turnratio, RUNNING, 0);
    }
    
    int Motor::off(){
        return set_output_state(0, REGULATED, MOTOR_SPEED, turnratio, RUN_IDLE, 0);
    }
    
    int Motor::reset_rotation(){
        return reset(1);
    }
    
    bool Motor::is_running(){
        get_output_state();
        if(run_state){
             return 1;              
        }
        else
             return 0;    
    }
    
    long int Motor::get_rotation(){
        get_output_state();
        return rotation_count;
    }

    int Motor::move_to(int speed, long int target){
        return move_to(speed,target,4);
    }
    
    int Motor::change_port(int new_port){
        port=new_port;
        return 1;    
    }
    
    int Motor::move_to(int speed, long int target,int tol){
        speed=abs(speed);
        long int current_pos= get_rotation();
        if(target>current_pos){
            on(speed);
            while(get_rotation()<=target){
                //wait                                
            }
            stop();                       
        }
        else{
            on(-speed);
            while(get_rotation()>=target){
               //wait                             
            }
            stop();                       
        }
        if( abs(target-get_rotation()) >tol ){
             move_to(5,target);
             
        }
        return 1;    
        
    }

    
    
/********* Work in progress******/


/*    int Motor::move_to(int speed, long int target){
        speed=abs(speed);
        int dir=1;
        long int slow_down=720;
        long int current_pos=get_rotation();
        if(target<current_pos){
            dir=-1;
        }    
        printf("target:%d\n",target);
        on(speed * dir);
        do{
             current_pos=get_rotation();
                 printf("Pos1 %d\n", current_pos);         
                 }while(target-slow_down > current_pos);
        set_output_state(0, BREAK | MOTOR_ON | REGULATED, reg_mode, turnratio, RAMP_DOWN, slow_down );
        do{
                 current_pos=get_rotation();
                 }while(target-15>current_pos);
        set_output_state(0, BREAK | MOTOR_ON| REGULATED, reg_mode, turnratio, RUNNING, 0);
        if(target<current_pos){
            dir=-1;
        }
        else{
            dir=1;     
        }
        on(10*dir);
        do{
                 current_pos=get_rotation();
        }while(target>current_pos);
        
        
    }*/
